#include "simucpp.hpp"
#include "utils.hpp"
#include "drawobj.hpp"
#include <iostream>
#include <cmath>
using namespace simucpp;
#ifdef SHOW_RESULT
#include "camera.h"
bool Pause() {
    static u8 state = 0x01;
    if (IsKeyDown(KEY_SPACE)) state |= 0x02;
    if (!IsKeyDown(KEY_SPACE) && (state & 0x02)) { state &=~ 0x02; state ^= 0x01; }
#if 1  // 抬起一次空格键暂停，再抬起一次继续
    if (state & 0x01) return true;
    else return false;
#else  // 抬起一次空格键走一步
    if ((state & 0x01) == (state >> 2)) { state ^= 0x04; return false; }
    else return true;
#endif
}
bool Shift_View() {
    static u8 state = 0x00;
    if (IsKeyDown(KEY_O)) state |= 0x02;
    if (!IsKeyDown(KEY_O) && (state & 0x02)) { state &=~ 0x02; state ^= 0x01; }
    if (state & 0x01) return true;
    else return false;
}
Vector3 Mat_to_Vector3(Mat m) {
    return (Vector3){float(m.at(0, 0)), float(m.at(1, 0)), float(m.at(2, 0))};
}
Matrix Mat33_to_Matrix(Mat m) {
    return (Matrix){
        (float)m.at(0, 0), (float)m.at(0, 1), (float)m.at(0, 2), 0,
        (float)m.at(1, 0), (float)m.at(1, 1), (float)m.at(1, 2), 0,
        (float)m.at(2, 0), (float)m.at(2, 1), (float)m.at(2, 2), 0,
        0, 0, 0, 1};
}
#endif  // SHOW_RESULT

// 四元数转旋转矩阵
Mat Quaternion_to_Rotation(Mat Q) {
    double q0 = Q.at(0, 0), q1 = Q.at(1, 0);
    double q2 = Q.at(2, 0), q3 = Q.at(3, 0);
    return Mat(3, 3, vecdble{
        q0*q0+q1*q1-q2*q2-q3*q3, 2*q1*q2-2*q0*q3, 2*q1*q3+2*q0*q2,
        2*q1*q2+2*q0*q3, q0*q0-q1*q1+q2*q2-q3*q3, 2*q2*q3-2*q0*q1,
        2*q1*q3-2*q0*q2, 2*q2*q3+2*q0*q1, q0*q0-q1*q1-q2*q2+q3*q3,
    });
}

int main(void) {
    Mat data = read_csv("./data.csv");
    Mat J(3, 3, vecdble{data.at(6,0), 0, 0, 0, data.at(7,0), 0, 0, 0, data.at(8,0)});
    Mat Jinv = J.inv();
    /* 初始化仿真器 */
    Simulator sim1;
    auto intQ = new MStateSpace(&sim1, BusSize(4, 1), true, "intQ");
    auto intW = new MStateSpace(&sim1, BusSize(3, 1), true, "intW");
    auto misoQ = new MFcnMISO(&sim1, BusSize(4, 1), "misoQ");
    auto misoW = new MFcnMISO(&sim1, BusSize(3, 1), "misoW");
    auto muxtau = new Mux(&sim1, BusSize(3, 1), "muxtau");
    UInput **intau = new UInput*[3];
    for (uint32_t i = 0; i < 3; i++) {
        intau[i] = new UInput(&sim1, "inw"+to_string(i));
        sim1.connectU(intau[i], muxtau, BusSize(i, 0));
    }
    sim1.connectM(intQ, misoQ);
    sim1.connectM(intW, misoQ);
    sim1.connectM(misoQ, intQ);
    sim1.connectM(intW, misoW);
    sim1.connectM(muxtau, misoW);
    sim1.connectM(misoW, intW);
    sim1.Set_SimStep(0.01);
    intQ->Set_InitialValue(Mat(vecdble{1, 0, 0, 0}));
    intW->Set_InitialValue(Mat(vecdble{data.at(3,0),data.at(4,0),data.at(5,0)}));
    intau[0]->Set_Function([&data](double t){ return data.at(0,0); });
    intau[1]->Set_Function([&data](double t){ return data.at(1,0); });
    intau[2]->Set_Function([&data](double t){ return data.at(2,0); });
    misoQ->Set_Function([](Mat *u){
        // u[0]为姿态四元数(4x1)，u[1]为固连系下的角速度向量(3x1)
        Mat W(4, 4, vecdble{  // 角速度向量w1对应的四元数乘法左矩阵(4x4)
            0, -u[1].at(0, 0), -u[1].at(1, 0), -u[1].at(2, 0),
            u[1].at(0, 0), 0, u[1].at(2, 0), -u[1].at(1, 0),
            u[1].at(1, 0), -u[1].at(2, 0), 0, u[1].at(0, 0),
            u[1].at(2, 0), u[1].at(1, 0), -u[1].at(0, 0), 0,
        });
        return 0.5*W*u[0];
    });
    misoW->Set_Function([&J, &Jinv](Mat *u){
        Vector3d omega = u[0];  // 固连系下的角速度向量(3x1)
        Vector3d tau = Mat(3, 1, vecdble{  // 固连系下的力矩向量(3x1)
            u[1].at(0, 0), u[1].at(1, 0), u[1].at(2, 0)
        });
        Vector3d ans = Jinv*(tau - (omega & (J*omega)));
        return Mat(ans);
    });
    sim1.Initialize();
    sim1.Simulate_FirstStep();

    Mat curq, curw, curR, omegaI, hmom;
#ifndef SHOW_RESULT
    /*测试*/
    while (1) {
        for (int i = 0; i < 10; i++)
            sim1.Simulate_OneStep();
    }

#else
    /*初始化场景*/
    Camera camera;
    SetWindowMonitor(1);
	SetConfigFlags(FLAG_MSAA_4X_HINT);
	SetConfigFlags(FLAG_FULLSCREEN_MODE);
    SetTargetFPS(50);
    InitGraph(0, 0, "Universe");
	Init_Camera(&camera);

    /*场景循环*/
    while (!WindowShouldClose()) {
        curq = intQ->Get_OutValue();  // 姿态四元数
        curR = Quaternion_to_Rotation(curq);  // 旋转矩阵
        curw = intW->Get_OutValue();  // 固连系下的角速度向量
        omegaI = curR * curw;  // 惯性系下的角速度向量
        hmom = curR * (J*curw);  // 惯性系下的角动量
        if (Shift_View())
            SetReferenceFrame(Mat33_to_Matrix(curR));
        else
            SetReferenceFrame(MatrixIdentity());

		Update_Camera(&camera);
        BeginDrawing();
        ClearBackground(BLACK);
        BeginMode3D(camera);
            DrawGrid(120, 2);
            Draw_Body(curR);  // 画刚体和固连坐标系
            DrawLine3D(Vector3Zero(), Mat_to_Vector3(omegaI), RED);  // 画惯性系下的角速度向量
            DrawLine3D(Vector3Zero(), Mat_to_Vector3(hmom), GREEN);  // 画惯性系下的角动量向量
        EndMode3D();
        EndDrawing();

        if (IsKeyDown(KEY_Q)) {
            sim1.Simulation_Reset();
        }
        if (Pause()) continue;
        for (int i = 0; i < 5; i++)
            sim1.Simulate_OneStep();
    }
    CloseGraph();
#endif
    return 0;
}
